import json
import rospy
import cv2

from datetime import datetime as dt

from rospy_message_converter import message_converter

from device_manager.arm_manager import ArmManager
from device_manager.sensor_manager import SensorManager

rospy.init_node("HAND_EYE_DATA_COLLECTION", anonymous=False)

am = ArmManager()
sm = SensorManager()

rospy.loginfo('Collecting data...')

# get current pose
pose = am.get_current_pose(raw=True)
pose_dict = message_converter.convert_ros_message_to_dictionary(pose)
pose_dict = pose_dict['pose']

# open file to write
filename = dt.strftime(dt.now(), '%Y-%m-%d-%H-%M-%S')
fp = open('../data/{}.json'.format(filename), 'w+')
fp.write(json.dumps(pose_dict)+'\n')
fp.close()

# get current image
rospy.sleep(1)
image = sm.get_image()
if len(image) > 0:
    sm.save_image(image, '../data/{}.jpg'.format(filename))

rospy.loginfo('Saved data in /data.')



